#include "obstacle_detector/collision_detector.h"
#include "cuda_runtime.h"

namespace obstacle_detector
{
__constant__ float vehicle_bbox_gpu_[8];


__device__ void transform2local(float *loc,float x,float y,float yaw)
{
	float new_x=loc[0]-x;
	float new_y=loc[1]-y;
	loc[0]=new_x*cos(yaw)+new_y*sin(yaw);
	loc[1]=-1*new_x*sin(yaw)+new_y*cos(yaw);
}

//计算p2和p1组成的向量 以及p3和p1组成的向量之间的叉积
__device__ float cross_product(float *p1,float *p2,float *p3)
{
	return (p2[0]-p1[0])*(p3[1]-p1[1])-(p2[1]-p1[1])*(p3[0]-p1[0]);
}

__device__ bool has_intersection(float *p1,float *p2,float *p3,float *p4)
{
	float c1=cross_product(p1,p2,p3);
	float c2=cross_product(p1,p2,p4);
	bool is_inter=c1*c2>0 ? false : true;
	c1=cross_product(p3,p4,p1);
	c2=cross_product(p3,p4,p2);
	is_inter=(c1*c2<=0 && is_inter) ? true : false;
	return is_inter;

}

__global__ void check_collision_kernel(const float *states,int state_num,
	const float *bbox,int bbox_num,uint8_t *bbox_index,int *min_collision_index)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=(state_num*4*bbox_num*4))
	{
		return;
	}
	int state_id=threadid/(bbox_num*4)/4;
	int state_corner_id=threadid/(bbox_num*4)%4;
	int state_next_corner_id=(state_corner_id+1)%4;
	int bbox_id=(threadid%(bbox_num*4))/4;
	int bbox_corner_id=(threadid%(bbox_num*4))%4;
	int bbox_next_corner_id=(bbox_corner_id+1)%4;

	float state_x=states[state_id*4+0];
	float state_y=states[state_id*4+1];
	float state_yaw=states[state_id*4+3];

	float corner1[2],corner2[2];
	corner1[0]=bbox[bbox_id*8+bbox_corner_id*2+0];
	corner1[1]=bbox[bbox_id*8+bbox_corner_id*2+1];
	corner2[0]=bbox[bbox_id*8+bbox_next_corner_id*2+0];
	corner2[1]=bbox[bbox_id*8+bbox_next_corner_id*2+1];
	// if (state_id==50)
	// {
	// 	printf ("has inter %d,%d: %d,%d,%f,%f,%f,%f,%f,%f,%f\n",state_id,bbox_id,state_corner_id,bbox_corner_id,
	// 		corner1[0],corner1[1],corner2[0],corner2[1],state_x,state_y,state_yaw);
	// }
	transform2local(corner1,state_x,state_y,state_yaw);
	transform2local(corner2,state_x,state_y,state_yaw);
	// float 
	bool has_inter=has_intersection(corner1,corner2,
		vehicle_bbox_gpu_+state_corner_id*2,vehicle_bbox_gpu_+state_next_corner_id*2);
	//当前框和障碍物框有碰撞
	if (has_inter)
	{
		// printf ("has inter %d,%d: %d,%d\n",state_id,bbox_id,state_corner_id,bbox_corner_id);
		atomicMin(min_collision_index,state_id*2+bbox_index[bbox_id]);
	}
	// if (state_id==50)
	// {
	// 	printf ("has inter %d,%d: %d,%d,%f,%f,%f,%f,%f,%f,%f\n",state_id,bbox_id,state_corner_id,bbox_corner_id,
	// 		corner1[0],corner1[1],corner2[0],corner2[1],state_x,state_y,state_yaw);
	// }



}


CollisionDetector::CollisionDetector(bool debug):
debug_(debug)
{
	cudaMalloc((void **)&predict_bbox_gpu_,sizeof(float)*10000*8);
	cudaMalloc((void **)&bbox_index_gpu_,sizeof(uint8_t)*10000);
	cudaMalloc((void **)&min_collision_index_gpu_,sizeof(int));
	cudaMalloc((void **)&state_gpu_,sizeof(float)*51*4);
	if (debug_)
	{
		debug_display_=std::make_unique<DebugDisplay>("collision_detector");
	}
}

float CollisionDetector::check_collision(
	const module_base::Location &loc,const std::vector<Object> &objects)
{
	// printf ("loc: %f,%f,%f\n",loc.x,loc.y,loc.yaw);
	float global_states[51*4];
	for (int i=0;i<51;i++)
	{
		float x=states_[i*4+0];
		float y=states_[i*4+1];
		float new_x=x*cos(-1*loc.yaw)+y*sin(-1*loc.yaw)+loc.x;
		float new_y=-1*x*sin(-1*loc.yaw)+y*cos(-1*loc.yaw)+loc.y;
		global_states[i*4+0]=new_x;
		global_states[i*4+1]=new_y;
		global_states[i*4+2]=states_[i*4+2];
		global_states[i*4+3]=states_[i*4+3]+loc.yaw;
	}
	// printf ("global_states: %f,%f,%f\n",global_states[50*4+0],global_states[50*4+1],
	// 	global_states[50*4+3]);
	cudaMemcpy(state_gpu_,global_states,sizeof(float)*51*4,
		cudaMemcpyHostToDevice);

	if (debug_)
	{
		std::vector<Object> lst_objects;
		debug_display_->show3Dbbox_compare(lst_objects,objects);
		debug_display_->set_state(global_states);
	}

	int valid_bbox_count=0;
	float *valid_bbox=new float [objects.size()*100*8];
	uint8_t *bbox_index=new uint8_t[objects.size()*100];
	for (int i=0;i<objects.size();i++)
	{
		// printf ("predict loc size: %d\n",objects[i].predict_locs.cols());
		//灯塔和锁站是静止障碍物 不需要进行碰撞检测
		if (objects[i].label==4 || objects[i].label==6)
		{
			continue;
		}

		if (objects[i].predict_locs.cols()<=1)
		{
			continue;
		}

		Eigen::MatrixXf global_bbox=objects[i].global_bbox.block(0,0,2,4);
		Eigen::Vector2f center=global_bbox.rowwise().mean();
		for (int j=0;j<objects[i].predict_locs.cols();j++)
		{
			Eigen::MatrixXf bbox=global_bbox.colwise()+(objects[i].predict_locs.col(j)-center);
			// if (j)
			bbox_index[valid_bbox_count]=j; 
			for (int k=0;k<4;k++)
			{
				valid_bbox[valid_bbox_count*8+k*2+0]=bbox(0,k);
				valid_bbox[valid_bbox_count*8+k*2+1]=bbox(1,k);
			}
			valid_bbox_count++;
		}
	}
	// printf ("valid_bbox_count: %d\n",valid_bbox_count);
	if (valid_bbox_count==0)
	{
		delete [] valid_bbox;
		delete [] bbox_index;
		return -1;
	}

	// valid_bbox[0]=0;
	// valid_bbox[1]=2;
	// valid_bbox[2]=2;
	// valid_bbox[3]=4;
	// valid_bbox[4]=4;
	// valid_bbox[5]=2;
	// valid_bbox[6]=2;
	// valid_bbox[7]=0;
	// valid_bbox_count=1;
	cudaMemcpy(predict_bbox_gpu_,valid_bbox,sizeof(float)*valid_bbox_count*8,
		cudaMemcpyHostToDevice);
	cudaMemcpy(bbox_index_gpu_,bbox_index,sizeof(uint8_t)*valid_bbox_count,
		cudaMemcpyHostToDevice);
	if (debug_)
	{
		debug_display_->set_predict_bbox(valid_bbox,valid_bbox_count);	
	}
	delete [] valid_bbox;
	delete [] bbox_index;

	cudaMemset(min_collision_index_gpu_,100,sizeof(int));
	// int min_collision_index;
	// cudaMemcpy(&min_collision_index,min_collision_index_gpu_,sizeof(int),
	// 	cudaMemcpyDeviceToHost);
	// printf ("min collision_index origin: %d\n",min_collision_index);

	int grid_dim=(51*valid_bbox_count*16+1024-1)/1024;
	check_collision_kernel<<<grid_dim,1024>>>(state_gpu_,51,
		predict_bbox_gpu_,valid_bbox_count,bbox_index_gpu_,
		min_collision_index_gpu_);

	int min_collision_index;
	cudaMemcpy(&min_collision_index,min_collision_index_gpu_,sizeof(int),
		cudaMemcpyDeviceToHost);
	// printf ("min collision_index: %d\n",min_collision_index);
	cuda_base::CUDA_CHECK_RETURN(cudaGetLastError());
	if (min_collision_index==1684300900) return -1;
	return min_collision_index*0.1;
}

void CollisionDetector::set_state(float *states)
{
	memcpy(states_,states,sizeof(float)*51*4);
}

void CollisionDetector::set_vehicle_param(
	const module_base::VehicleParam &vehicle_param)
{
	vehicle_param_=vehicle_param;
	if (debug_)
	{
		debug_display_->set_vehicle_param(vehicle_param_);
	}
	float vehicle_length=vehicle_param_.v_length;
	float vehicle_width=vehicle_param_.v_width;
	float back_edge_to_center=vehicle_param_.back_edge_to_center;
	float vehicle_bbox[8]={-1*back_edge_to_center,vehicle_width/2,
							vehicle_length-back_edge_to_center,vehicle_width/2,
							vehicle_length-back_edge_to_center,-1*vehicle_width/2,
							-1*back_edge_to_center,-1*vehicle_width/2};
	

	cudaMemcpyToSymbol(vehicle_bbox_gpu_, vehicle_bbox, sizeof(float) * 8 );
}
	
}